#pragma once

#include <opencv2\opencv.hpp>
#include "ImageMatch.h"
#include "math_3d.h"
#include "..\MathAux\Matrix\matrix.h"

cv::Point2d	operator*(const cv::Point2d& rhs, const cv::Point2d& lhs);
double* GetMatrixDataAsDouble(cv::Mat A);

template<typename T>
static T* CastToBundlerVec(double* p)
{
	return reinterpret_cast<T*>(p);
}



namespace Utils
{

#define EYE(_sz) cv::Mat::eye((_sz), (_sz), cv::DataType<double>::type)
#define ZEROS(_row, _col) cv::Mat::zeros((_row), (_col), cv::DataType<double>::type)
#define CV_MAT(m, i,j) (m.at<double>(i,j))
#define INIT_MAT(i, j , ...) (cv::Mat_<double>(i,j) << __VA_ARGS__);
#define INIT_MAT_ARRAY(i,j,_arr) (cv::Mat(i, j, cv::DataType<double>::type, _arr).clone())

	struct SSVD
	{
		cv::Mat u;
		cv::Mat w;
		cv::Mat vt;
	};
	

	cv::Mat Diag(int size, double* vals);

	cv::Point3d Point3dFromArray(double* p);

	cv::Point2d Point2dFromArray( double* p);

	SSVD ComputeSVD(cv::Mat A, bool bUseOpenCV);

	int match_t();

	cv::Point2d MoveFromScreenSpace(const cv::Point2d& p, int width, int height);

	cv::Mat GetCameraMatrixFromMatchPoints(const CImageMatch* pMatch);

	CImageMatch* match(IplImage* img1, IplImage* img2);

	void CreateCrossMatrix(cv::Vec3d vec, cv::Mat& CrossMat);

	cv::Mat VecToMat(const cv::Vec3d& v);

	Matrix4f ConvertCVtoM3D( cv::Mat cvMat);

	cv::Mat ExpandToHVector(cv::Mat vec);
	
	cv::Mat Project3D(cv::Mat src);

	cv::Mat ConvertCamCenterToTranslation(cv::Mat R, cv::Mat c);

	double Dot(cv::Mat a, cv::Mat b);

	double Dot(cv::Point2d a, cv::Point2d b);

	void InvertDistortion(int n_in, int n_out, double r0, double r1, 
		double *k_in, double *k_out);

	template<typename T>
	void PrintMat(const cv::Mat& M)
	{
		UINT LineCounter = 0;
		for (cv::MatConstIterator_<T> it = M.begin<T>(); it != M.end<T>(); it++)
		{

			T ele = *it;
			CONSOLE("%.9e ", ele);
 			LineCounter++;
			if ((LineCounter % M.cols) == 0)
			{
				CONSOLE("\n");
			}

		}
	
	}

	template<UINT size>
	cv::Vec<double, size> MatToVec(const cv::Mat& M)
	{
		cv::Vec<double, size> ret;
		for (UINT i = 0; i < size; i++)
		{
			ret[i] = M.at<double>(0,i);
		}
		return ret;
	}
}


template<typename T>
T Clamp(T _val, T _min, T _max)
{
	return max(min(_val, _max), _min);
}